# import can
import socket
import json

import glob
import os
import sys
import time
import math
try:
    sys.path.append(glob.glob('C:/carla/PythonAPI/carla/dist/carla-*%d.%d-%s.egg' % (
        sys.version_info.major,
        sys.version_info.minor,
        'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0])
except IndexError:
    pass

import carla 


def generate_id_0x530(angel=0, gear='p', steer_speed=0):
    ss = bin(steer_speed)[2:]

    while len(ss) < 9:
        ss = '0' + ss

    s1 = hex(int(ss[0:7], 2))[2:]

    while len(s1) < 2:
        s1 = '0' + s1

    s2 = hex(int(ss[7:], 2))[2:]

    g = {
        0: '001',
        -1: '010',
        "n": '011',
        1: '100',
        2: '100',
        3: '100',
        4: '100',
        5: '100'
    }
    t1 = hex(angel - 600)
    t2 = hex(int(g[gear], 2))
    frame = '00 ' + t1[4:6] + ' 0' + t1[3] + ' 0' + s2 + ' ' + s1 + ' ' + t2[2] + '0 00 01'

    frame_list = frame.split(' ')
    data = []
    for i in range(len(frame_list)):
        data.append(int(frame_list[i], 16))

    # can_msg = can.Message(arbitration_id=0x530, is_extended_id=True, data=data, dlc=8)
    can_dict = {
            "id":0x530,
            "data":data
        }
    data = json.dumps(can_dict).encode()
    print(data)
    return data


def generate_id_0x448(gear='n'):
    g = {
        0: '8',
        -1: '7',
        "n": '6',
        1: '5',
        2: '5',
        3: '5',
        4: '5',
        5: '5'
    }
    t = g[gear]
    frame = "0" + t + " 00 00 00 00"

    frame_list = frame.split(' ')
    data = []
    for i in range(len(frame_list)):
        data.append(int(frame_list[i], 16))

    # can_msg = can.Message(arbitration_id=0x530, is_extended_id=True, data=data, dlc=8)
    can_dict = {
            "id":0x448,
            "data":data
        }
    data = json.dumps(can_dict).encode()
    print(data)
    return data

def generate_id_0x532(acc=0, brake=0):
    s = bin(acc)[2:]
    b = bin(brake)[2:]

    while len(s) < 7:
        s = '0' + s

    while len(b) < 7:
        b = '0' + b

    # s2 = hex(int(s1[0:3], 2))[2:]
    # s3 = hex(int(s1[3:7], 2))[2:]
    #
    # b2 = hex(int(b1[0:2], 2) + 4)[2:]
    # b3 = hex(int(b1[2:6], 2))[2:]
    # b4 = hex(int(b1[6], 2))[2:]
    #
    # if brake != 0:
    #     frame = '00 00 00 ' + b3 + b4 + ' 0' + b2 + ' 00 00 00'
    # else:
    #     frame = '00 00 ' + s3 + '0 0' + s2 + ' 44 00 00 00'
    s1 = s[0:3]
    s2 = s[3:7]

    b1 = b[0:2]
    b2 = b[2:6]
    b3 = b[6]

    x1 = hex(int(s2, 2))[2:]
    x2 = hex(int(b2, 2))[2:]
    x3 = hex(int(b3 + s1, 2))[2:]
    x4 = hex(int(b1, 2))[2:]

    frame = '00 00 ' + x1 + '0 ' + x2 + x3 + ' 0' + x4 + ' 00 00 00'

    frame_list = frame.split(' ')
    data = []
    for i in range(len(frame_list)):
        data.append(int(frame_list[i], 16))

    # can_msg = can.Message(arbitration_id=0x534, is_extended_id=True, data=data, dlc=8)

    can_dict = {
            "id":0x532,
            "data":data
        }
    data = json.dumps(can_dict).encode()
    print(data)
    return data

# beamLight 1近2远
# steeringLight 1左2右
# hazardLight 雾灯
# widthLight 宽灯

def generate_id_0x534(hazardLight=0, beamLight=0, steeringLight=0, widthLight=0, backupLight=0, brakLight=0,
                      doorlock=0):
    if hazardLight == 1:
        t = 1
    else:
        t = 2

    if brakLight == 1:
        t0 = 4
    else:
        t0 = 8

    s1 = hex(t + t0)[2]

    if beamLight == 1:  # 近光
        t1 = 4
    elif beamLight == 2:  # 远光
        t1 = 8
    else:
        t1 = 12

    if steeringLight == 1:
        t2 = 1
    elif steeringLight == 2:
        t2 = 2
    else:
        t2 = 3

    s2 = hex(t1 + t2)[2]

    if widthLight == 1:
        t3 = 1
    else:
        t3 = 2

    if backupLight == 1:
        t4 = 4
    else:
        t4 = 8
    s3 = hex(t3 + t4)[2]

    if doorlock == 1:
        s4 = '4'
    else:
        s4 = '8'

    frame = '00 ' + s1 + s2 + ' ' + '0' + s3 + ' 00 0' + s4 + ' 00 00 00'

    frame_list = frame.split(' ')
    data = []
    for i in range(len(frame_list)):
        data.append(int(frame_list[i], 16))

    # can_msg = can.Message(arbitration_id=0x534, is_extended_id=True, data=data, dlc=8)

    can_dict = {
        "id":0x534,
        "data":data
    }
    data = json.dumps(can_dict).encode()
    print(data)
    return data

def generate_id_0x56F(speed=0, gear=0):
    sp = bin(speed * 100)[2:]
    while len(sp) < 15:
        sp = '0' + sp

    sp1 = hex(int(sp[0:7], 2))[2:]
    sp2 = hex(int(sp[7:15], 2))[2:]
    while len(sp1) < 2:
        sp1 = '0' + sp1
    while len(sp2) < 2:
        sp2 = '0' + sp2

    if gear == -1:
        s = '6'
    elif gear == 0:
        s = '0'
    elif gear == 1:
        s = '1'
    elif gear == 2:
        s = '2'
    elif gear == 3:
        s = '3'
    elif gear == 4:
        s = '4'
    elif gear == 5:
        s = '5'

    frame = '00 00 00 00 00 ' + sp2 + ' ' + sp1 + ' 0' + s

    frame_list = frame.split(' ')
    data = []
    for i in range(len(frame_list)):
        data.append(int(frame_list[i], 16))

    # can_msg = can.Message(arbitration_id=0x534, is_extended_id=True, data=data, dlc=8)

    can_dict = {
        "id":0x56F,
        "data":data
    }
    data = json.dumps(can_dict).encode()
    print(data)
    return data


# def can_msg_to_dict(can_msg):
#
#     dict_can = {
#         "id": can_msg.arbitration_id,
#         "data": can_msg.data,
#         "dlc": can_msg.dlc,
#         "is_extend": can_msg.is_extended_id,
#     }
#     return dict_can


if __name__ == '__main__':

    # Connect to client
    client = carla.Client('127.0.0.1', 2000)
    client.set_timeout(2.0)

    world = client.get_world()
    current_map = world.get_map()
    actors = world.get_actors()

    car = 0

    for actor in actors:
        if actor.type_id == 'vehicle.charger2020.charger2020':
            car = actor
    
    if car == 0:
        print("Error: cannot found main car!")
        exit
    
    address = ('192.168.31.101', 31501)  # 服务端地址和端口
    s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)

    # can_id, can_data = generate_id_0x534()

    # can_dict = {
    #     "id":can_id,
    #     "data":can_data
    # }
    # print(can_dict)
    # data = json.dumps(can_dict).encode()
    # print(data)

# NONE
# Position  位置
# LowBeam   近光
# HighBeam  远光
# Brake     刹车
# RightBlinker  右转
# LeftBlinker   左转
# Reverse   倒车
# Fog       雾灯
# Interior  内部灯

    while True:
        vehicleControl = car.get_control()
        throttle = int(vehicleControl.throttle * 100)
        print(throttle)
        steer = int(vehicleControl.steer * -100)
        brake = int(vehicleControl.brake * 100)
        reverse = vehicleControl.reverse 
        gear = vehicleControl.gear
        
        v = car.get_velocity()
        speed = int(3.6 * math.sqrt(v.x**2 + v.y**2 + v.z**2))  # km/h


        s.sendto(generate_id_0x530(steer, gear, 100), address)
        s.sendto(generate_id_0x532(throttle, brake), address)
        s.sendto(generate_id_0x448(gear), address)
        s.sendto(generate_id_0x56F(speed,gear), address)


        lightState = car.get_light_state()
        beamLight= 0
        if lightState & lightState.LowBeam > 0: beamLight = 1
        if lightState & lightState.HighBeam > 0: beamLight = 2
        steeringLight = 0
        if lightState & lightState.LeftBlinker > 0: steeringLight = 1
        if lightState & lightState.RightBlinker > 0: steeringLight = 2
        hazardLight = 0
        if lightState & lightState.Fog > 0: hazardLight = 1
        widthLight = 0
        if lightState & lightState.Position > 0: widthLight = 1
        backupLight = 0
        if lightState & lightState.Reverse > 0: backupLight = 1
        brakLight = 0
        if lightState & lightState.Brake > 0: brakLight = 1
        s.sendto(generate_id_0x534(hazardLight, beamLight, steeringLight, widthLight, backupLight, brakLight, 0), address)


        # car.set_light_state(lightState.NONE)

        time.sleep(0.3)

    # s.sendto(generate_id_0x534(), address)
